import sensor, image, time
from pyb import UART

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_windowing((120,100))
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)
sensor.skip_frames(time = 2000)


uart=UART(3,9600)
uart.init(9600, bits=8, parity=None, stop=1)


clock = time.clock()


th1 = (54,80,-21,-9,0,19)
th2 = (99,100,-6,0,0,2)


def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    blobs = img.find_blobs([th1,th2])
    if blobs:
        max_blob = find_max(blobs)
        #img.draw_rectangle(max_blob[0:4],color=(255,0,0))
        uart.write(bytearray([0X3A,0X5C,max_blob.cx(),max_blob.cy()]))
        #print(max_blob.cx(),max_blob.cy())
